﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Threading;
using System.IO;
using System.Diagnostics;

namespace R10App
{
    public partial class MainForm : Form
    {
        static uint canHandle;
        Thread RXThread;
        bool RXActive;
        const int IOBaseAddr = 0x0010000;
        const int IMUBaseAddr = 0x0030000;
        const int XBEEBaseAddr = 0x0040000;

        private JoystickHandler joy = new JoystickHandler();

        // data
        SDataStructure DataStructure = new SDataStructure();

        public struct SDataStructure
        {
            public int ReceivedPackets;
            public int ReceiveErrors;
            public int RXThreadTicks;

            public int TransmitPackets;
            public int TransmitErrors;

            // data
            // Sensors - I/O Board
            public int ModeActual;
            public int Fail;
            public float Voltage;
            public float PWMOut1;
            public float PWMOut2;
            public float LoopTime;
            public float LoopTimeMAX;
            public double TotalTime;

            public float dYaw;
            public float Yaw;
            public float MagX;
            public float MagY;
            public float MagZ;
            public float MagYaw;
        };

        public MainForm()
        {
            InitializeComponent();
        }

        private void MainForm_Load(object sender, EventArgs e)
        {
            toolStripStatusLabelCANAdapter.Text = "No adapters";

            // find adapters
            StringBuilder buf = new StringBuilder(32);
            int cnt;

            // Fill the listbox with data
            cnt = LAWICEL.canusb_getFirstAdapter(buf, 32);
            if (cnt > 0) toolStripStatusLabelCANAdapter.Text = "Adapter found: " + buf.ToString();

            joy.Init(this.Handle);
        } 

        private void buttonOpen_Click(object sender, EventArgs e)
        {
            canHandle = LAWICEL.canusb_Open(IntPtr.Zero,
                                        LAWICEL.CAN_BAUD_1M,
                                        LAWICEL.CANUSB_ACCEPTANCE_CODE_ALL,
                                        LAWICEL.CANUSB_ACCEPTANCE_MASK_ALL,
                                        0);

            if (canHandle == 0) MessageBox.Show("Open Failed");
            else
            {
                // OK
                buttonOpen.Enabled = false;
                buttonClose.Enabled = true;

                // init structures
                DataStructure = new SDataStructure();

                // create receive thread
                RXActive = true;
                RXThread = new Thread(new ThreadStart(RXThreadProc));
                //RXThread.Priority = ThreadPriority.AboveNormal;
                RXThread.IsBackground = true;
                RXThread.Start();

                timerGUI.Enabled = true;
            }
        }

        private void buttonClose_Click(object sender, EventArgs e)
        {
            timerGUI.Enabled = false;

            // Close the CANUSB
            int res = LAWICEL.canusb_Close(canHandle);
            if (LAWICEL.ERROR_CANUSB_OK == res)
            {
                // OK
                buttonOpen.Enabled = true;
                buttonClose.Enabled = false;
                RXActive = false;
            }
            else
            {
                MessageBox.Show("Close Failed");
            }
        }

        void RXThreadProc()
        {
            // open log file
            StreamWriter logFile = File.CreateText(Application.StartupPath + "\\LogFile.txt");
            logFile.WriteLine("TimeSec Voltage PWM1 PWM2");
            //logFile.WriteLine("TimeSec Omega Yaw MagX MagY MagZ");
            Stopwatch timer = new Stopwatch();
            timer.Start();

            while (RXActive)
            {
                // Read one CAN message
                LAWICEL.CANMsg msg = new LAWICEL.CANMsg();

                int rv = LAWICEL.canusb_Read(canHandle, out msg);
                if (LAWICEL.ERROR_CANUSB_OK == rv)
                {
                    // data OK, convert to float
                    byte[] bytes = BitConverter.GetBytes(msg.data);
                    float data = BitConverter.ToSingle(bytes, 0);
                    double dataDbl = BitConverter.ToDouble(bytes, 0);

                    lock (this)
                    {
                        // IOBoard
                        uint baseAddr = IOBaseAddr;                        
                        if (msg.id == 0x311 + baseAddr) DataStructure.Voltage = data;
                        if (msg.id == 0x120 + baseAddr) DataStructure.PWMOut1 = data;
                        if (msg.id == 0x121 + baseAddr) DataStructure.PWMOut2 = data;

                        // IMU Board
                        baseAddr = IMUBaseAddr;
                        if (msg.id == 0x302 + baseAddr) DataStructure.dYaw = -data;
                        if (msg.id == 0x312 + baseAddr) DataStructure.Yaw = data;
                        if (msg.id == 0x340 + baseAddr) DataStructure.MagX = data;
                        if (msg.id == 0x341 + baseAddr) DataStructure.MagY = data;
                        if (msg.id == 0x342 + baseAddr)
                        {
                            DataStructure.MagZ = data;
                            double magYaw = Math.Atan2(DataStructure.MagY - 0.0867f, DataStructure.MagZ + 0.29f) / 6.28318548f;
                            magYaw = (magYaw - Math.Floor(magYaw)) * 6.28318548f;
                            DataStructure.MagYaw = (float)(magYaw *180 / Math.PI);
                        }

                        // XBee Board
                        baseAddr = XBEEBaseAddr;
                        if (msg.id == 0x400 + baseAddr) DataStructure.ModeActual = (int)data;
                        if (msg.id == 0x401 + baseAddr) DataStructure.Fail = (int)data;
                        if (msg.id == 0x350 + baseAddr) DataStructure.LoopTime = data;
                        if (msg.id == 0x351 + baseAddr) DataStructure.TotalTime = dataDbl;
                        if (msg.id == 0x352 + baseAddr) DataStructure.LoopTimeMAX = data;

                        DataStructure.ReceivedPackets++;
                    }

                    // log data
                    double currentMS = (double)timer.ElapsedTicks/(double)Stopwatch.Frequency * 1000.0;
                    // Log on PWMOut1 (last chunk)
                    if (msg.id == 0x120 + IOBaseAddr)
                    {
                        //string line = string.Format("{0} {1} {2} {3} {4} {5} {6} {7} {8}", currentMS / 1000, DataStructure.Voltage, DataStructure.PWMOut1, DataStructure.PWMOut2, DataStructure.Current1, DataStructure.Power1, DataStructure.Force1FLT, DataStructure.Moment1FLT, DataStructure.SpeedRPM1);
                        //logFile.WriteLine(line);
                        // MAG Save
                        //string line = string.Format("{0} {1} {2} {3} {4} {5}", currentMS / 1000, DataStructure.OmegaZ, DataStructure.Yaw, DataStructure.MagX, DataStructure.MagY, DataStructure.MagZ);
                        //logFile.WriteLine(line);
                    }
                }
                else if (LAWICEL.ERROR_CANUSB_NO_MESSAGE == rv)
                {
                    // no messages, sleep
                    Thread.Sleep(1);
                    lock (this)
                    {
                        DataStructure.RXThreadTicks++;
                    }
                }

                else
                {
                    // error!
                    lock (this)
                    {
                        DataStructure.ReceiveErrors++;
                    }
                }
            }

            logFile.Close();
        }

        public void SendCANMessage(uint id, float testVar)
        {
            // Send one message with extended id
            LAWICEL.CANMsg msg = new LAWICEL.CANMsg();
            msg.id = id;
            msg.len = 8;
            msg.flags = LAWICEL.CANMSG_EXTENDED;
            byte[] data = new byte[8];
            BitConverter.GetBytes(testVar).CopyTo(data, 0);
            msg.data = BitConverter.ToUInt64(data, 0);
            int rv = LAWICEL.canusb_Write(canHandle, ref msg);
            if (LAWICEL.ERROR_CANUSB_OK == rv)
            {
                // OK
                lock (this)
                {
                    DataStructure.TransmitPackets++;
                }
            }
            else if (LAWICEL.ERROR_CANUSB_TX_FIFO_FULL == rv)
            {
                lock (this)
                {
                    DataStructure.TransmitErrors++;
                }
            }
            else
            {
                lock (this)
                {
                    DataStructure.TransmitErrors++;
                }
            }
        }

        private void timerGUI_Tick(object sender, EventArgs e)
        {
            // update GUI
            // 1. get copy
            SDataStructure copyStruct;
            lock (this)
            {
                copyStruct = DataStructure;
            }

            // 2. set values
            textBoxTXRXFrames.Text = string.Format("{0}/{1}", copyStruct.TransmitPackets, copyStruct.ReceivedPackets);
            textBoxTXRXErrors.Text = string.Format("{0}/{1}", copyStruct.TransmitErrors, copyStruct.ReceiveErrors);
            textBoxRXTicks.Text = copyStruct.RXThreadTicks.ToString();


            textBoxModeActual.Text = string.Format("{0}", copyStruct.ModeActual);
            textBoxBattVoltage.Text = string.Format("{0:f1} V", copyStruct.Voltage);
            textBoxPWMOut1.Text = string.Format("{0:f0}", copyStruct.PWMOut1);
            textBoxPWMOut2.Text = string.Format("{0:f0}", copyStruct.PWMOut2);
            textBoxLoopMS.Text = string.Format("{0:f2} ms", copyStruct.LoopTime);
            textBoxLoopMAXMS.Text = string.Format("{0:f2} ms", copyStruct.LoopTimeMAX);
            textBoxTotalTime.Text = string.Format("{0:f2} s", copyStruct.TotalTime);

            textBoxMagX.Text = string.Format("{0:f3}", copyStruct.MagX);
            textBoxMagY.Text = string.Format("{0:f3}", copyStruct.MagY);
            textBoxMagZ.Text = string.Format("{0:f3}", copyStruct.MagZ);
            textBoxMagYaw.Text = string.Format("{0:f3}", copyStruct.MagYaw);
            textBoxdYaw.Text = string.Format("{0:f1} deg/s", copyStruct.dYaw / Math.PI * 180);
            textBoxYaw.Text = string.Format("{0:f1} deg", copyStruct.Yaw / Math.PI * 180);

            

            // update mode
            int mode = (int)copyStruct.ModeActual;
            switch (mode)
            {
                case 0: //off
                    pictureOff.BackColor = Color.Lime;
                    picturePWMThrust.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Red;
                    buttonOff.Enabled = true;
                    buttonPWMThrust.Enabled = true;
                    buttonAltMode.Enabled = true;
                    break;

                case 1: // PWM Trust
                    pictureOff.BackColor = Color.Red;
                    picturePWMThrust.BackColor = Color.Lime;
                    pictureAltMode.BackColor = Color.Red;
                    buttonOff.Enabled = true;
                    buttonPWMThrust.Enabled = false;
                    buttonAltMode.Enabled = false;
                    break;

                case 2: // Alt Mode
                    pictureOff.BackColor = Color.Red;
                    picturePWMThrust.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Lime;
                    buttonOff.Enabled = true;
                    buttonPWMThrust.Enabled = false;
                    buttonAltMode.Enabled = false;
                    break;

                default:
                    pictureOff.BackColor = Color.Red;
                    picturePWMThrust.BackColor = Color.Red;
                    pictureAltMode.BackColor = Color.Red;
                    break;
            }

            // update status
            if( copyStruct.Fail == 0)
            {
                // OK
                pictureStatus.BackColor = Color.Green;
                buttonAckError.Enabled = false;
            }
            else
            {
                // FAIL
                pictureStatus.BackColor = Color.Red;
                buttonAckError.Enabled = true;
            }

            // 3. read inputs (joy), TODO!!!
            JoyState js =joy.Poll();
            if (js == null) return; 
            double PWMSet = js.Throttle * 1000.0 + 1000;
            double YawRef = double.Parse(textBoxYawRef.Text.Split(' ')[0]);
            if (Math.Abs(js.AxisX) > 0.2)
            {
                YawRef += (js.AxisX * 0.05);
            }
            if (YawRef < 0) YawRef = Math.PI * 2;
            if (YawRef > Math.PI * 2) YawRef = 0;

            textBoxPWMSet.Text = string.Format("{0:f0}", PWMSet);
            textBoxYawRef.Text = string.Format("{0:0.000} /{1:0.0}", YawRef, YawRef*180/Math.PI);

            // 4. send messages
            bool silentMode = false;
            if (!silentMode)
            {
                uint baseAddr = XBEEBaseAddr;
                SendCANMessage(0x0102 + baseAddr, (float)PWMSet);
                SendCANMessage(0x0103 + baseAddr, (float)YawRef);
            }

            // 5. Read status (reset error???)
            //int bla = LAWICEL.canusb_Status(canHandle);
        }

        private void buttonModeSelect_Click(object sender, EventArgs e)
        {
            uint modeAddress = XBEEBaseAddr + 0x0100;

            if (sender == buttonOff)
            {
                SendCANMessage(modeAddress, 0);
            }
            else if (sender == buttonPWMThrust) SendCANMessage(modeAddress, 1);
            else if (sender == buttonAltMode) SendCANMessage(modeAddress, 2);
        }     

        private void buttonAckError_Click(object sender, EventArgs e)
        {
            uint ackErrorAddress = XBEEBaseAddr + 0x0101;
            SendCANMessage(ackErrorAddress, 1);
        }
    }
}
